#pragma once
#define Hchassis_Version 20220809

/*--------------------------------------------------------------------------
Module  : ActionUnit.h
Author  : LQ
Created : 2020.2.21
brief   : 本类是对ActionUnit类进行封装。加进了底盘直线前进功能。
          底盘控制中, 可以去掉稳定的摇杆小误差;具有auto-hold功能。
----------------------------------------------------------------------------*/
/*
change log:
【2022.5.14】增加auto_hold功能。
【2022.8.9】 增加优化底盘控制函数数学模型。

*/

#include "../ActionUnit/ActionUnit.h"
#include "joystick_value_process.h"

/////////////////////////
/*
  1.每转脉冲数:
  每转脉冲数是马达输出轴转1圈, 编码器的数值。
  定义每转脉冲数为rtpr(raw ticks per revolution)
   红齿 36:1 - rtpr=1800
   绿齿 18:1 - rtpr=900
   蓝齿  6:1 - rtpr=300

  2.单位转换公式:
  (raw) / rtpr =  (rev)
  (rev) * 360 =  (deg)

  3.路程与编码器
  （rev） / _external_gear_reduction_ratio  *   _wheel_travel  =  dist

*/

/////////////////////////////////////////////////////////////////////////////////////////////////////////
class Hchassis {
private:
  JoystickValueProcess process;
  ActionUnit *p_au_l = NULL;
  ActionUnit *p_au_r = NULL;

private:
  double _external_gear_reduction_ratio; //外部齿轮减速比,默认

  double wheel_diameter_in; //轮子直径（英寸）
  /*
  _wheel_travel:
  4    in 大轮: 319.2 mm
  3.25 in 中轮：259.3 mm
  2.75 in 小轮：219.4 mm
  */
  double _wheel_travel;      //轮子周长,默认4英寸直径轮
  double _track_width = 324; //左右轮距
  double _wheel_base = 320;  //前后轮轴距
  brakeType set_brake_type = brakeType::coast; //默认刹停模式

public:
  //使用马达组构建底盘
  Hchassis(ActionUnit &l, ActionUnit &r, double wheel_diameter_in,
           double external_gear_ratio, float track_width = 324) {
    p_au_l = &l;
    p_au_r = &r;
    _wheel_travel = wheel_diameter_in * PI * 25.4; // mm
    _external_gear_reduction_ratio = external_gear_ratio;
    if (ActionUnit_Version >= 20220414) {
      l.set_shaft_disengaged_detecting_working(false); //开启联动脱轴检测
      r.set_shaft_disengaged_detecting_working(false); //开启联动脱轴检测
    }
    set_stopping(brakeType::brake); //默认停转模式
  }
  ////////////////////////////////////////////////////////////////////////////////
  double get_actual_velocity(int side, int mtr_index = 0,
                             velocityUnits units_v = velocityUnits::pct);
  double get_enc(int side = -1, rotationUnits unit_r = rotationUnits::deg);
  double get_dist_mm(int side = -1, int mtr_index = 0); //-1 左侧，右侧
  double get_dist_mm_avg();

  void reset_enc();

private:
  bool auto_hold_turned_on = false;
  void set_stopping_without_saving(brakeType brake_type);

public:
  void set_stopping(brakeType brake_type); //设定刹停模式
  char get_stopping_char();
  void mtr_stop(brakeType brake_type);
  void stop();

  void LR_set_voltage(double mv_pct_l, double mv_pct_r);
  void LR_set_speed(double v_pct_l, double v_pct_r);
  void FT_set_voltage(double forward_pct, double turn_pct);
  void FT_set_speed(double forward_pct, double turn_pct);
  ////////////////////////////////////////////////////////////////
private:
  //注意需要使左右两侧编码器初始值相同,不然会造成旋转
  void fast_forward_enc_absolute(double rotation, double speed,
                                 int timeout_ms = 0,
                                 Block waitForCompletion = Block::yes,
                                 rotationUnits units_r = rotationUnits::deg,
                                 velocityUnits units_v = velocityUnits::pct);
  void fast_forward_enc_relative(double rotation, double speed,
                                 int timeout_ms = 0,
                                 Block waitForCompletion = Block::yes,
                                 rotationUnits units_r = rotationUnits::deg,
                                 velocityUnits units_v = velocityUnits::pct);

public:
  //注意需要使左右两侧编码器初始值相同,不然会造成旋转
  void fast_forward_dist_absolute(double dist, double speed, int timeout_ms = 0,
                                  Block waitForCompletion = Block::yes,
                                  distanceUnits units_d = distanceUnits::mm,
                                  velocityUnits units_v = velocityUnits::pct);
  void fast_forward_dist_relative(double dist, double speed, int timeout_ms = 0,
                                  Block waitForCompletion = Block::yes,
                                  distanceUnits units_d = distanceUnits::mm,
                                  velocityUnits units_v = velocityUnits::pct);
  ////////////////////////////////////////////////////////////////
private:
  void fast_turn_enc_absolute(double rotation, double speed, int timeout_ms = 0,
                              Block waitForCompletion = Block::yes,
                              rotationUnits units_r = rotationUnits::deg,
                              velocityUnits units_v = velocityUnits::pct);
  void fast_turn_enc_relative(double rotation, double speed, int timeout_ms = 0,
                              Block waitForCompletion = Block::yes,
                              rotationUnits units_r = rotationUnits::deg,
                              velocityUnits units_v = velocityUnits::pct);

public:
  void fast_turn_dist_absolute(double dist, double speed, int timeout_ms = 0,
                               Block waitForCompletion = Block::yes,
                               distanceUnits units_d = distanceUnits::mm,
                               velocityUnits units_v = velocityUnits::pct);
  void fast_turn_dist_relative(double dist, double speed, int timeout_ms = 0,
                               Block waitForCompletion = Block::yes,
                               distanceUnits units_d = distanceUnits::mm,
                               velocityUnits units_v = velocityUnits::pct);
  //判断底盘是不是在跑编码器,包括fast forward 和fast turn
  bool is_acting_enc_fast_ft() {
    return p_au_l->is_act_enc_running() || p_au_r->is_act_enc_running();
  }
  ////////////////////////////////////////////////////////////////////////////////
  enum ctrlMode { m_voltage, m_velocity };
  void handle(double chx_forward, double chx_turn, ctrlMode mode = m_voltage,
              double max_forward_pct = 100, double max_turn_pct = 100,
              bool auto_hold = true);

private:
  double distance_to_rev(double distance, distanceUnits units);
  double rev_to_raw(double r, gearSetting s = gearSetting::ratio18_1);
  double rev_to_distance_mm(double r);

  ///////////////////////
private:
  float get_inner_spd(float vo, float radius_center, float tw);

public:
  void curve_move();
  void arc_move(int side, int spd_pct_outter_arc, double outter_arc_dist_mm,
                float radius_center);

  // stable_forward_dist_relative
private:
  bool forward_pid_adjust_force_stop = false; //强制停止
  bool forward_pid_adjust_running = false;    //标志，正在运行

  struct {
    bool run_done; //状态标志，是否完成
    double aim_dist_mm;
    double max_pwr;
    int timeout;
    bool waitForCompletion;
    bool b_remove_gearspace;
    bool b_reset_enc;
    double max_adjust_pwr;
    double hold_angle;
  } arg_stable_forward_pid_adjust_dist_relative;

  static void forward_pid_adjust_dist_relative_data_cb_static(void *p_this);

  double remove_gearspace(int dir);
  void point_move_relative(int dist_relative = 5, int timeout = 100);

public:
  // stable 模式会缓加速、缓减速，以及使用陀螺仪纠正前进后退的方向
  void stable_forward_dist_relative(double aim_dist_mm, double max_pwr = 100,
                                    int timeout = 0,
                                    Block waitForCompletion = Block::yes,
                                    double hold_angle = 74000,
                                    bool b_remove_gearspace = true,
                                    bool b_reset_enc = true,
                                    double max_adjust_pwr = 10);
  bool is_stable_forward_dist_running() {
    return !arg_stable_forward_pid_adjust_dist_relative.run_done;
  }
  void stable_forward_dist_stop() { stop(); }
};
